
import DH from "./DH";
import * as math from "mathjs";
import { getTMatrix, r2d } from './Common';

const EPS = 0.00001;

function urInverse_bk( pose, model='UR5' ) {
  
  if( pose.length !== 6 ) return;

  let dh;
  let dof = 6;

  switch(model){
    case 'UR5':
      dh = DH.UR5;
      dof = 6;
      break;
    default:
      break;
  }

  if( !dh ) return;

  const T = getTMatrix( pose );

  if( !T ) return;

  const nx = T[0][0];
  const ny = T[1][0];
  const nz = T[2][0];

  const ox = T[0][1];
  const oy = T[1][1];
  const oz = T[2][1];

  const ax = T[0][2];
  const ay = T[1][2];
  const az = T[2][2];

  const px = T[0][3];
  const py = T[1][3];
  const pz = T[2][3];

  const d1 = dh[0][2];
  const d4 = dh[3][2];
  const d5 = dh[4][2];
  const d6 = dh[5][2];

  const a2 = dh[1][1];
  const a3 = dh[2][1];

  var m = d6 * ay - py;
  var n = d6 * ax - px;

  // 无解
  var tmp = m*m + n*n - d4*d4;
  if( tmp < 0 ) return;

  // theta1
  const theta1 = [];
  theta1[0] = Math.atan2(m, n) - Math.atan2(d4, Math.sqrt(tmp));
  theta1[1] = Math.atan2(m, n) - Math.atan2(d4, -Math.sqrt(tmp));
  
  // theta5
  const theta5 = [];
  for( let i = 0, il = theta1.length; i < il; i++ ){
    tmp = ax*Math.sin(theta1[i]) - ay*Math.cos(theta1[i]);
    if( tmp > 1 ) return;
    const r = Math.acos(tmp);
    theta5.push(r);
    theta5.push(-r);
  }

  // theta6
  const theta6=[];
  for( let i = 0, il = theta1.length; i < il; i++ ) {
    m = nx*Math.sin(theta1[i]) - ny*Math.cos(theta1[i]);

    for( let j = 0, jl = theta5.length; j < jl; j++ ){
      const s5 = Math.sin(theta5[j]);
      if( Math.abs(s5) >  EPS && Math.abs(m / s5) <= 1 ) {
        const r = Math.acos( m / s5 );
        theta6.push(r);
        theta6.push(-r);
      }
    }
  }

  // console.log('s6:', theta6.map(r=>Math.sin(r)));

  // theta3
  const theta2 = [];
  const theta3 = [];
  const theta4 = [];

  for( let i = 0, il = theta1.length; i < il; i++ ){
    for( let j = 0, jl = theta6.length; j < jl; j++ ) {
      const s1 = Math.sin(theta1[i]);
      const c1 = Math.cos(theta1[i]);
      const s6 = Math.sin(theta6[j]);
      const c6 = Math.cos(theta6[j]);
      m = d5*(s6*(nx*c1+ny*s1)+c6*(ox*c1+oy*s1))-d6*(ax*c1+ay*s1)+px*c1+py*s1;
      n = pz - d1 - az*d6 + d5*(oz*c6 +nz*s6);
      if( m*m + n*n <= (a2+a3)*(a2+a3) ) {
        const r = Math.acos((m*m + n*n - a2*a2 - a3*a3)/(2*a2*a3));
        theta3.push(r);       
        theta3.push(-r);

        [r, -r].map((t3)=>{
          const s3 = Math.sin(t3);
          const c3 = Math.cos(t3);
          tmp = a3*a3*s3*s3 + (a3*c3+a2)*(a3*c3+a2) - m*m;
          if( tmp >= 0 ){
            const t2_1 = Math.atan2(a3*c3+a2, a3*s3) - Math.atan2(m, Math.sqrt(tmp));
            const t2_2 = Math.atan2(a3*c3+a2, a3*s3) + Math.atan2(m, Math.sqrt(tmp));
            theta2.push(t2_1);
            theta2.push(t2_2);

            const ty = -s6*(nx*c1 + ny*s1) - c6*(ox*c1 + oy*s1);
            const tx = oz*c6 + nz*s6;
            const t4_1 = Math.atan2(ty, tx) - t2_1 - t3;
            const t4_2 = Math.atan2(ty, tx) - t2_2 - t3;
            theta4.push(t4_1);
            theta4.push(t4_2);
          }
        })
        
      }
    }
  } 

  console.log( 'theta1:', theta1.map(r=>r2d(r)) );
  console.log( 'theta2:', theta2.map(r=>r2d(r)) );
  console.log( 'theta3:', theta3.map(r=>r2d(r)) );
  console.log( 'theta4:', theta4.map(r=>r2d(r)) );
  console.log( 'theta5:', theta5.map(r=>r2d(r)) );
  console.log( 'theta6:', theta6.map(r=>r2d(r)) );

}

function urInverse( pose, model='UR5' ) {
  
  if( pose.length !== 6 ) return;

  let dh;

  switch(model){
    case 'UR5':
      dh = DH.UR5;
      break;
    default:
      break;
  }

  if( !dh ) return;

  const T = getTMatrix( pose );

  if( !T ) return;

  const nx = T[0][0];
  const ny = T[1][0];
  const nz = T[2][0];

  const ox = T[0][1];
  const oy = T[1][1];
  const oz = T[2][1];

  const ax = T[0][2];
  const ay = T[1][2];
  const az = T[2][2];

  const px = T[0][3];
  const py = T[1][3];
  const pz = T[2][3];

  const d1 = dh[0][2];
  const d4 = dh[3][2];
  const d5 = dh[4][2];
  const d6 = dh[5][2];

  const a2 = dh[1][1];
  const a3 = dh[2][1];

  var m = d6 * ay - py;
  var n = d6 * ax - px;

  // 无解
  var tmp = m*m + n*n - d4*d4;
  if( tmp < 0 ) return;

  // theta1
  const theta1 = [];
  theta1[0] = Math.atan2(m, n) - Math.atan2(d4, Math.sqrt(tmp));
  theta1[1] = Math.atan2(m, n) - Math.atan2(d4, -Math.sqrt(tmp));
  
  // theta5
  const theta5 = [];
  for( let i = 0, il = theta1.length; i < il; i++ ){
    tmp = ax*Math.sin(theta1[i]) - ay*Math.cos(theta1[i]);
    if( tmp > 1 ) return;
    const r = Math.acos(tmp);
    theta5.push(r);
    theta5.push(-r);
  }

  // theta6
  const theta6=[];
  m = [nx*Math.sin(theta1[0]) - ny*Math.cos(theta1[0]),nx*Math.sin(theta1[1]) - ny*Math.cos(theta1[1])];
  const s5 = [Math.sin(theta5[0]),Math.sin(theta5[1]),Math.sin(theta5[2]),Math.sin(theta5[3])];
  
  if( Math.abs(s5[0]) >  EPS && Math.abs(m[0]/ s5[0]) <= 1 ) {
      const r = Math.acos( m[0] / s5[0] );
      theta6.push(r);
  }

  if( Math.abs(s5[1]) >  EPS && Math.abs(m[0]/ s5[1]) <= 1 ) {
      const r = Math.acos( m[0] / s5[1] );
      theta6.push(r);
  }

  if( Math.abs(s5[2]) >  EPS && Math.abs(m[1]/ s5[2]) <= 1 ) {
      const r = Math.acos( m[1] / s5[2] );
      theta6.push(r);
  }

  if( Math.abs(s5[3]) >  EPS && Math.abs(m[1]/ s5[3]) <= 1 ) {
      const r = Math.acos( m[1] / s5[3] );
      theta6.push(r);
  }

  //theta3
  const theta3 = [];
  const s1 = [Math.sin(theta1[0]),Math.sin(theta1[1])];
  const c1 = [Math.cos(theta1[0]),Math.cos(theta1[1])];
  const s6 = [Math.sin(theta6[0]),Math.sin(theta6[1]),Math.sin(theta6[2]),Math.sin(theta6[3])];
  const c6 = [Math.cos(theta6[0]),Math.cos(theta6[1]),Math.cos(theta6[2]),Math.cos(theta6[3])];
  m = [
      d5*(s6[0]*(nx*c1[0]+ny*s1[0])+c6[0]*(ox*c1[0]+oy*s1[0]))-d6*(ax*c1[0]+ay*s1[0])+px*c1[0]+py*s1[0],
      d5*(s6[1]*(nx*c1[0]+ny*s1[0])+c6[1]*(ox*c1[0]+oy*s1[0]))-d6*(ax*c1[0]+ay*s1[0])+px*c1[0]+py*s1[0],
      d5*(s6[2]*(nx*c1[1]+ny*s1[1])+c6[2]*(ox*c1[1]+oy*s1[1]))-d6*(ax*c1[1]+ay*s1[1])+px*c1[1]+py*s1[1],
      d5*(s6[3]*(nx*c1[1]+ny*s1[1])+c6[3]*(ox*c1[1]+oy*s1[1]))-d6*(ax*c1[1]+ay*s1[1])+px*c1[1]+py*s1[1],
  ];
  n = [
      pz - d1 - az*d6 + d5*(oz*c6[0] +nz*s6[0]),
      pz - d1 - az*d6 + d5*(oz*c6[1] +nz*s6[1]),
      pz - d1 - az*d6 + d5*(oz*c6[2] +nz*s6[2]),
      pz - d1 - az*d6 + d5*(oz*c6[3] +nz*s6[3]),
  ];
  if( m[0]*m[0] + n[0]*n[0] <= (a2+a3)*(a2+a3) ) {
    const r = Math.acos((m[0]*m[0] + n[0]*n[0] - a2*a2 - a3*a3)/(2*a2*a3));
    theta3.push(r);       
    theta3.push(-r);
  }
  if( m[1]*m[1] + n[1]*n[1] <= (a2+a3)*(a2+a3) ) {
      const r = Math.acos((m[1]*m[1] + n[1]*n[1] - a2*a2 - a3*a3)/(2*a2*a3));
      theta3.push(r);       
      theta3.push(-r);
  }
  if( m[2]*m[2] + n[2]*n[2] <= (a2+a3)*(a2+a3) ) {
      const r = Math.acos((m[2]*m[2] + n[2]*n[2] - a2*a2 - a3*a3)/(2*a2*a3));
      theta3.push(r);       
      theta3.push(-r);
  }
  if( m[3]*m[3] + n[3]*n[3] <= (a2+a3)*(a2+a3) ) {
      const r = Math.acos((m[3]*m[3] + n[3]*n[3] - a2*a2 - a3*a3)/(2*a2*a3));
      theta3.push(r);       
      theta3.push(-r);
  }

  //theta2
  const s3 = [];
  const c3 = [];
  for(var i=0;i<theta3.length;i++){
      s3.push(Math.sin(theta3[i]));
      c3.push(Math.cos(theta3[i]));
  }
  const s2 = [
      ((a3*c3[0]+a2)*n[0]-a3*s3[0]*m[0])/(a2*a2+a3*a3+2*a2*a3*c3[0]),
      ((a3*c3[1]+a2)*n[0]-a3*s3[1]*m[0])/(a2*a2+a3*a3+2*a2*a3*c3[1]),
      ((a3*c3[2]+a2)*n[1]-a3*s3[2]*m[1])/(a2*a2+a3*a3+2*a2*a3*c3[2]),
      ((a3*c3[3]+a2)*n[1]-a3*s3[3]*m[1])/(a2*a2+a3*a3+2*a2*a3*c3[3]),
      ((a3*c3[4]+a2)*n[2]-a3*s3[4]*m[2])/(a2*a2+a3*a3+2*a2*a3*c3[4]),
      ((a3*c3[5]+a2)*n[2]-a3*s3[5]*m[2])/(a2*a2+a3*a3+2*a2*a3*c3[5]),
      ((a3*c3[6]+a2)*n[3]-a3*s3[6]*m[3])/(a2*a2+a3*a3+2*a2*a3*c3[6]),
      ((a3*c3[7]+a2)*n[3]-a3*s3[7]*m[3])/(a2*a2+a3*a3+2*a2*a3*c3[7]),
  ];
  const c2 = [
      (m[0]+a3*s3[0]*s2[0])/(a3*c3[0]+a2),
      (m[0]+a3*s3[1]*s2[1])/(a3*c3[1]+a2),
      (m[1]+a3*s3[2]*s2[2])/(a3*c3[2]+a2),
      (m[1]+a3*s3[3]*s2[3])/(a3*c3[3]+a2),
      (m[2]+a3*s3[4]*s2[4])/(a3*c3[4]+a2),
      (m[2]+a3*s3[5]*s2[5])/(a3*c3[5]+a2),
      (m[3]+a3*s3[6]*s2[6])/(a3*c3[6]+a2),
      (m[3]+a3*s3[7]*s2[7])/(a3*c3[7]+a2),
  ];
  const theta2=[];
  for(let i=0;i<s2.length;i++){
      theta2.push(Math.atan2(s2[i],c2[i]));
  }

  //theta234
  const theta234=[];
  const s234 = [
      -Math.sin(theta6[0])*(nx*Math.cos(theta1[0])+ny*Math.sin(theta1[0]))-Math.cos(theta6[0])*(ox*Math.cos(theta1[0])+oy*Math.sin(theta1[0])),
      -Math.sin(theta6[1])*(nx*Math.cos(theta1[0])+ny*Math.sin(theta1[0]))-Math.cos(theta6[1])*(ox*Math.cos(theta1[0])+oy*Math.sin(theta1[0])),
      -Math.sin(theta6[2])*(nx*Math.cos(theta1[1])+ny*Math.sin(theta1[1]))-Math.cos(theta6[2])*(ox*Math.cos(theta1[1])+oy*Math.sin(theta1[1])),
      -Math.sin(theta6[3])*(nx*Math.cos(theta1[1])+ny*Math.sin(theta1[1]))-Math.cos(theta6[3])*(ox*Math.cos(theta1[1])+oy*Math.sin(theta1[1]))
  ];
  const c234 = [
      oz*Math.cos(theta6[0])+nz*Math.sin(theta6[0]),
      oz*Math.cos(theta6[1])+nz*Math.sin(theta6[1]),
      oz*Math.cos(theta6[2])+nz*Math.sin(theta6[2]),
      oz*Math.cos(theta6[3])+nz*Math.sin(theta6[3]),
  ];
  for(let i=0;i<s234.length;i++){
      theta234.push(Math.atan2(s234[i],c234[i]));
  }

  //theta4
  const theta4 = [];
  theta4.push(theta234[0] - theta3[0] - theta2[0]);
  theta4.push(theta234[0] - theta3[1] - theta2[1]);
  theta4.push(theta234[1] - theta3[2] - theta2[2]);
  theta4.push(theta234[1] - theta3[3] - theta2[3]);
  theta4.push(theta234[2] - theta3[4] - theta2[4]);
  theta4.push(theta234[2] - theta3[5] - theta2[5]);
  theta4.push(theta234[3] - theta3[6] - theta2[6]);
  theta4.push(theta234[3] - theta3[7] - theta2[7]);
  
  //8组解
  const solution = [
      [theta1[0], theta2[0], theta3[0], theta4[0], theta5[0], theta6[0]],
      [theta1[0], theta2[1], theta3[1], theta4[1], theta5[0], theta6[0]],
      [theta1[0], theta2[2], theta3[2], theta4[2], theta5[1], theta6[1]],
      [theta1[0], theta2[3], theta3[3], theta4[3], theta5[1], theta6[1]],
      [theta1[1], theta2[4], theta3[4], theta4[4], theta5[2], theta6[2]],
      [theta1[1], theta2[5], theta3[5], theta4[5], theta5[2], theta6[2]],
      [theta1[1], theta2[6], theta3[6], theta4[6], theta5[3], theta6[3]],
      [theta1[1], theta2[7], theta3[7], theta4[7], theta5[3], theta6[3]]
  ]
 
  for(let i=0;i<solution.length;i++){
    for(var j=0;j<solution[i].length;j++){

      if (solution[i][j] >= Math.PI){
        solution[i][j] = solution[i][j] - Math.PI * 2;
      }
      if (solution[i][j] <=- Math.PI){
        solution[i][j] = solution[i][j] + Math.PI * 2;
      }
      // 返回弧度制
      // solution[i][j] = r2d( solution[i][j] ); 
    }
  }

  return solution;

}

function findNearest( jointValues, solution ) {
  var nearest = Number.MAX_VALUE;
  var nearestIndex = -1;
  var dist = 0;
  for( var i = 0; i < solution.length; i ++ ) {
    if( jointValues.length !== solution[i].length ) continue;
    dist = 0;
    for( var j = 0; j < jointValues.length; j++ ) {
      dist += (jointValues[j] - solution[i][j]) * (jointValues[j] - solution[i][j]);
    }

    if( !isNaN(dist) && dist < nearest ) {
      nearest = dist;
      nearestIndex = i;
    }

  }

  return solution[nearestIndex];
}

function getInverse( pose , model='UR5') {
  switch(model){
    case 'UR5':
      return urInverse(pose);
    default:
      return null;
  }
}
export {
  getInverse,
  findNearest,
}